#include "AStar.h"
#include "../core/heuristics.h"
#include "../core/BinaryHeap.h"
#include "../PathFinder.h"
#include "../core/AStarNode.h"

AStar::AStar(void)
{
	openList = new BinaryHeap();
}


AStar::~AStar(void)
{
	if (openList)
		delete openList;
}

AStarNode* AStar::calculatePath( PathFinder* finder,AStarNode* startNode,AStarNode* endNode,std::vector<AStarNode*>& toClear )
{
	openList->clear();
	startNode->g = 0;
	startNode->h = finder->getHeuristic(endNode,startNode);
	startNode->f = startNode->g + startNode->h;
	openList->push(startNode);
	toClear.push_back(startNode);
	startNode->opened = true;

	while(!openList->empty())
	{
		AStarNode* n = openList->pop();
		n->closed = true;
		if (n == endNode) return n;
		std::vector<AStarNode*> neighbours;
		finder->grid->getNeighbours(neighbours,n,finder->allowDiagonal,finder->tunnel);
		
		for (size_t i=0;i<neighbours.size();++i)
		{
			AStarNode* neighbour = neighbours[i];
			if (!neighbour->closed)
			{
				toClear.push_back(neighbour);
				if (!neighbour->opened)
				{
					neighbour->g = Huge;
					neighbour->parent = NULL;
				}
				updateVertex(finder,openList,n,neighbour,endNode);
			}
		}
	}

	return NULL;
}

void AStar::updateVertex( PathFinder* finder,BinaryHeap* openList,AStarNode* node,AStarNode* neighbour,AStarNode* endNode )
{
	int oldG = neighbour->g;
	computeCost(node,neighbour,finder);
	if (neighbour->g < oldG)
	{
		if (!neighbour->isBlock)
		{
			if (neighbour->opened)
				neighbour->opened = false;
			neighbour->h = finder->getHeuristic(endNode,neighbour);
			neighbour->f = neighbour->g + neighbour->h;
			openList->push(neighbour);
			neighbour->opened = true;
		}
	}
}

void AStar::computeCost( AStarNode* node,AStarNode* neighbour,PathFinder* finder )
{
	float mCost = euclidian(neighbour,node);
	if (node->g + mCost < neighbour->g)
	{
		neighbour->parent = node;
		neighbour->g = node->g + mCost;
	}
}
